#include <iostream>
#include <math.h>
#define KP 0.05
#define KI 0.55
#define KD 0.07

typedef struct Indicator
{
    double ActiveValue;
    double TargetValue;
    double OutputMax;
    double CurrError;
    double LastError;
    double PrevError;
    double Increment;
    double Transform;
} PID_Indicator;

PID_Indicator PID_Initial()
{
    PID_Indicator indicator;
    indicator.ActiveValue = 0;
    indicator.TargetValue = 0;
    indicator.OutputMax = 0;
    indicator.CurrError = 0;
    indicator.LastError = 0;
    indicator.PrevError = 0;
    indicator.Increment = 0;
    indicator.Transform = 0;
    return indicator;
}

void PID_SetParameters(PID_Indicator *indicator, float ActiveValue, float TargetValue, float OutputMax)
{
    indicator->ActiveValue = ActiveValue;
    indicator->TargetValue = TargetValue;
    indicator->OutputMax = OutputMax;
}

double PID_Calculate(PID_Indicator *indicator)
{
    indicator->PrevError = indicator->LastError;
    indicator->LastError = indicator->CurrError;
    indicator->CurrError = indicator->TargetValue - indicator->ActiveValue;

    double Proportion = KP * (indicator->CurrError - indicator->LastError);
    double Intergral = KI * indicator->CurrError;
    double Differential = KD * (indicator->CurrError - 2 * indicator->LastError + indicator->PrevError);
    double Increment = Proportion + Intergral + Differential;

    double Output = 0;
    Output = Increment < -indicator->OutputMax ? -indicator->OutputMax : Increment;
    Output = Output > indicator->OutputMax ? indicator->OutputMax : Output;

    return Output;
}

double PID_Transform(PID_Indicator *indicator)
{
    double TransformValue, TransformValue_;

    TransformValue = ((int)(((-indicator->Increment / 40) + 15) * 10 + 0.5)) / 10.0;

    TransformValue_ = (TransformValue < 15 && TransformValue > 14.5) ? 14.5 : TransformValue;
    TransformValue_ = (TransformValue > 15 && TransformValue < 15.5) ? 15.5 : TransformValue;

    printf("\t TransformValue = %.4lf", TransformValue);

    return TransformValue_;
}

double PID_AlgorithmCall(PID_Indicator *indicator)
{
    indicator->Increment = PID_Calculate(indicator);
    indicator->ActiveValue = indicator->Increment + indicator->ActiveValue;
    printf("Error = %.4lf \t Increment = %.4lf ", indicator->CurrError, indicator->Increment);
    indicator->Transform = PID_Transform(indicator);
    printf("\t ServoValue = %.4lf\n", indicator->Transform);
    return indicator->Transform;
}

int main()
{
    PID_Indicator indicator = PID_Initial();
    PID_SetParameters(&indicator, 200, 0, 200);

    int cout = 0;

    while (1)
    {
        printf("Iteration %d: ", cout);
        indicator.Transform = PID_AlgorithmCall(&indicator);
        cout++;
        if ((float)indicator.CurrError == 0.0000)
            break;
    }
}
